/*  -*- mode: c++; coding: utf-8; c-file-style: "stroustrup"; -*-

    Copyright 2008 Asier Aguirre <asier.aguirre@gmail.com>
    This file is part of Intromit.

    Intromit is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    Intromit is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with Intromit.  If not, see <http://www.gnu.org/licenses/>.
*/

#include "matrix.hh"

#include <math.h>
#include <stdlib.h>
#include <string.h>

CLASS_INIT(matrix); // MATRIX CLASS

matrix::matrix()
{
    CLASS_CTOR(matrix);
    ctor();
}

matrix::~matrix()
{
    CLASS_DTOR(matrix);
    dtor();
}

void matrix::ctor()
{
    m=n=-1;
    mxn=-1;
    c=NULL;

    // temporal memory
    minor=NULL;
    n_minor=max_minor=0;
}

void matrix::dtor()
{
    if(c) c=(double*)realloc(c, 0);
    m=n=-1;
    mxn=-1;

    if(minor) minor=(double*)realloc(minor, 0);
    n_minor=max_minor=0;
}

int matrix::init(int m_/*rows*/, int n_/*columns*/)
{
    if(m!=m_ || n!=n_)
    {
	m=m_;
	n=n_;
	mxn=m*n;
	c=(double*)realloc(c, mxn*sizeof(double));
	identity();
    }
    return 1;
}

void matrix::identity()
{
    for(int j=0, k=0; j<n; j++) for(int i=0; i<m; i++, k++) c[k]=(i==j)?1.0:0.0;
}

void matrix::set_coefs(double *coefs)
{
    memmove(c, coefs, mxn*sizeof(double));
}

void matrix::get_coefs_copy(double *coefs)
{
    memcpy(coefs, c, mxn*sizeof(double));
}

void matrix::vect_mult(double *factor, double *res)
{
    for(int i=0; i<m; i++) res[i]=0.0;
    for(int j=0, k=0; j<n; j++) for(int i=0; i<m; i++, k++) res[i]+=c[k]*factor[j];
}

bool matrix::matrix_mult(matrix *m, matrix *res)
{
    if(m->m != m->n) ERROR_RX(false, "factor is not square matrix");
    if(res->m != res->n) ERROR_RX(false, "result is not square matrix");
    if(m->n != res->n || m->n != n) ERROR_RX(false, "diferent sizes");
    return matrix_mult(m->c, res->c);
}

bool matrix::matrix_mult(double *coefs, double *res)
{
    if(n!=m) ERROR_RX(false, "not square matrix in multiplication");
    for(int i=0; i<n; i++)
    {
	for(int j=0; j<n; j++)
	{
	    double sum=0;
	    for(int k=0; k<n; k++)
		sum+=c[i+k*m]*coefs[k+j*m];
	    res[i+j*m]=sum;
	}
    }
    return true;
}

bool matrix::invert(matrix *r)
{
    // checks
    if(!r->init(m, n)) ERROR_RX(false, "init result matrix");
    return invert(r->c);
}

bool matrix::invert(double *result)
{
    // checks
    if(m!=n) ERROR_RX(false, "should be square matrix");

    // determinant
    double inv_det=determinant();
    if(inv_det==0.0) ERROR_RX(false, "determinant is zero => not inversible");
    inv_det=1.0/inv_det;

    // cofactor inversion method
    for(int j=0, k=0; j<n; j++) for(int i=0; i<m; i++, k++) result[k]=inv_det*cofactor(j, i);

    // ok
    return true;
}

double matrix::determinant()
{
    if(m!=n) ERROR_R0("should be square matrix");

    double det=0;
    for(int i=0; i<n; i++)
	if(c[i])
	    det+=c[i]*cofactor(i, 0);

    return det;
}

int matrix::minor_matrix(double *m, int s, int ii, int jj)
{
    int k=n_minor;
    for(int j=0; j<s; j++)
	if(j!=jj)
	    for(int i=0; i<s; i++)
		if(i!=ii)
		    minor[k++]=m[i+j*s];
    return k;
}

double matrix::cofactor_recur(double *m, int s, int ii, int jj)
{
    // recur end
    if(s==2)
    {
	double det=m[(ii | (jj<<1)) ^ 0x3];
	return ((ii^jj)&1)?-det:det;
    }

    // minor matrix
    int n_minor_save=n_minor;
    n_minor=minor_matrix(m, s, ii, jj);
    double *min=&minor[n_minor_save];

    // calculate minor matrix determinant
    double det=0;
    for(int i=0; i<s-1; i++)
	if(min[i])
	    det+=min[i]*cofactor_recur(min, s-1, i, 0);

    n_minor=n_minor_save;
    return ((ii^jj)&1)?-det:det;
}

double matrix::cofactor(int ii, int jj)
{
    if(m!=n) ERROR_R0("should be square matrix");
    if(n<=1) return 0.0;

    // don't do reallocs during recursion
    int recur_tam=0;
    for(int i=1; i<=n; i++) recur_tam+=i*i;
    minimum_minor_size(recur_tam);

    return cofactor_recur(c, n, ii, jj);
}

void matrix::dump(FILE *arch, int colours, char *tab)
{
    for(int i=0; i<m; i++)
    {
	fprintf(arch, "%s", tab);
	for(int j=0; j<n; j++)
	{
	    if(colours)
	    {
		if(i==j) fprintf(arch, WHITE); else fprintf(arch, RESET);
	    }
	    fprintf(arch, "%12.6f ", c[i+j*m]);
	}
	if(colours) fprintf(arch, RESET);
	fprintf(arch, "\n");
    }
}

void matrix::dump_octave_stdout()
{
    FILE *arch=stdout;
    fprintf(arch, "[");
    for(int i=0; i<m; i++)
    {
	fprintf(arch, "[");
	for(int j=0; j<n; j++)
	    fprintf(arch, "%e,", c[i+j*m]);
	fprintf(arch, "];");
    }
    fprintf(arch, "]\n");
}


void matrix::minimum_minor_size(int s)
{
    if(s>max_minor)
    {
	max_minor=s;
	minor=(double*)realloc(minor, max_minor*sizeof(double));
    }
}



CLASS_INIT(matrix4x4); // MATRIX4X4 CLASS

matrix4x4::matrix4x4()
{
    CLASS_CTOR(matrix4x4);
    ctor();
}

matrix4x4::matrix4x4(double coefs[16])
{
    CLASS_CTOR(matrix4x4);
    set_coefs(coefs);
}

matrix4x4::~matrix4x4()
{
    CLASS_DTOR(matrix4x4);
    dtor();
}

void matrix4x4::ctor()
{
    identity();
}

void matrix4x4::dtor()
{
}

void matrix4x4::identity()
{
    for(int j=0, k=0; j<4; j++) for(int i=0; i<4; i++, k++) c[k]=(i==j)?1.0:0.0;
}

void matrix4x4::set_coefs(double coefs[16])
{
    memmove(c, coefs, 4*4*sizeof(double));
}

void matrix4x4::get_coefs_copy(double coefs[16])
{
    memcpy(coefs, c, 4*4*sizeof(double));
}

void matrix4x4::vect_mult(double factor[4], double res[4])
{
    for(int i=0; i<4; i++) res[i]=0.0;
    for(int j=0, k=0; j<4; j++) for(int i=0; i<4; i++, k++) res[i]+=c[k]*factor[j];
}

void matrix4x4::vect_mult(double c[16], double factor[4], double res[4])
{
    for(int i=0; i<4; i++) res[i]=0.0;
    for(int j=0, k=0; j<4; j++) for(int i=0; i<4; i++, k++) res[i]+=c[k]*factor[j];
}

void matrix4x4::vect_mult_3x3_1(double c[16], double factor[3], double res[3])
{
    res[0]=factor[0]*c[0] + factor[1]*c[4] + factor[2]*c[8]  + c[12];
    res[1]=factor[0]*c[1] + factor[1]*c[5] + factor[2]*c[9]  + c[13];
    res[2]=factor[0]*c[2] + factor[1]*c[6] + factor[2]*c[10] + c[14];
}

void matrix4x4::vect_mult_3x3_0(double c[16], double factor[3], double res[3])
{
    res[0]=factor[0]*c[0] + factor[1]*c[4] + factor[2]*c[8];
    res[1]=factor[0]*c[1] + factor[1]*c[5] + factor[2]*c[9];
    res[2]=factor[0]*c[2] + factor[1]*c[6] + factor[2]*c[10];
}

void matrix4x4::vect_mult_3x3_1_inv(double factor[3], double c[16], double res[3])
{
    res[0]=factor[0]*c[0] + factor[1]*c[1] + factor[2]*c[2]  + c[3];
    res[1]=factor[0]*c[4] + factor[1]*c[5] + factor[2]*c[6]  + c[7];
    res[2]=factor[0]*c[8] + factor[1]*c[9] + factor[2]*c[10] + c[11];
}

void matrix4x4::vect_mult_3x3_0_inv(double factor[3], double c[16], double res[3])
{
    res[0]=factor[0]*c[0] + factor[1]*c[1] + factor[2]*c[2];
    res[1]=factor[0]*c[4] + factor[1]*c[5] + factor[2]*c[6];
    res[2]=factor[0]*c[8] + factor[1]*c[9] + factor[2]*c[10];
}

void matrix4x4::matrix_mult(matrix4x4 *m, matrix4x4 *res)
{
    matrix_mult(m->c, res->c);
}

void matrix4x4::matrix_mult(double coefs[16], double res[16])
{
    for(int i=0; i<4; i++)
    {
	for(int j=0; j<4; j++)
	{
	    double sum=0;
	    for(int k=0; k<4; k++)
		sum+=c[i+(k<<2)]*coefs[k+(j<<2)];
	    res[i+(j<<2)]=sum;
	}
    }
}

void matrix4x4::matrix_mult_inv_order(double coefs[16], double res[16])
{
    for(int i=0; i<4; i++)
    {
	for(int j=0; j<4; j++)
	{
	    double sum=0;
	    for(int k=0; k<4; k++)
		sum+=coefs[i+(k<<2)]*c[k+(j<<2)];
	    res[i+(j<<2)]=sum;
	}
    }    
}

void matrix4x4::rotation_coefs(double a, double v[3], double coefs[16])
{
    // normalize vector
    double vmod=sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]);
    v[0]/=vmod;
    v[1]/=vmod;
    v[2]/=vmod;

    double c=cos(a), c1=1-c;
    double s=sin(a);

    coefs[0]=v[0]*v[0]*c1+c;
    coefs[1]=v[1]*v[0]*c1+v[2]*s;
    coefs[2]=v[0]*v[2]*c1-v[1]*s;

    coefs[4]=v[0]*v[1]*c1-v[2]*s;
    coefs[5]=v[1]*v[1]*c1+c;
    coefs[6]=v[1]*v[2]*c1+v[0]*s;

    coefs[8]=v[0]*v[2]*c1+v[1]*s;
    coefs[9]=v[1]*v[2]*c1-v[0]*s;
    coefs[10]=v[2]*v[2]*c1+c;

    coefs[3]=coefs[7]=coefs[11]=coefs[12]=coefs[13]=coefs[14]=0;
    coefs[15]=1;
}

void matrix4x4::scale_coefs(double xyz, double coefs[16])
{
    coefs[1]=coefs[2]=coefs[3]=
	coefs[4]=coefs[6]=coefs[7]=
	coefs[8]=coefs[9]=coefs[11]=
	coefs[12]=coefs[13]=coefs[14]=0;
    coefs[0]=coefs[5]=coefs[10]=xyz;
    coefs[15]=1;
}

void matrix4x4::scale_coefs(double xyz[3], double coefs[16])
{
    coefs[1]=coefs[2]=coefs[3]=
	coefs[4]=coefs[6]=coefs[7]=
	coefs[8]=coefs[9]=coefs[11]=
	coefs[12]=coefs[13]=coefs[14]=0;
    coefs[0]=xyz[0];
    coefs[5]=xyz[1];
    coefs[10]=xyz[2];
    coefs[15]=1;
}

void matrix4x4::translate_coefs(double orig[3], double coefs[16])
{
    coefs[0]=coefs[5]=coefs[10]=coefs[15]=1;
    coefs[1]=coefs[2]=coefs[3]=
	coefs[4]=coefs[6]=coefs[7]=
	coefs[8]=coefs[9]=coefs[11]=0;
    coefs[12]=orig[0];
    coefs[13]=orig[1];
    coefs[14]=orig[2];
}

void matrix4x4::rotation(double angle, double vector[3])
{
    rotation_coefs(angle, vector, c);
}

void matrix4x4::scale(double xyz)
{
    scale_coefs(xyz, c);
}

void matrix4x4::scale(double xyz[3])
{
    scale_coefs(xyz, c);
}

void matrix4x4::translate(double orig[3])
{
    translate_coefs(orig, c);
}

void matrix4x4::do_rotation(double angle, double vector[3])
{
    double coefs[16], res[16];
    rotation_coefs(angle, vector, coefs);
    matrix_mult(coefs, res);
    set_coefs(res);
}

void matrix4x4::do_rotation(double angle, double x, double y, double z)
{
    double t[3]={x, y, z};
    do_rotation(angle, t);
}

void matrix4x4::do_scale(double xyz)
{
    double coefs[16], res[16];
    scale_coefs(xyz, coefs);
    matrix_mult(coefs, res);
    set_coefs(res);
}

void matrix4x4::do_scale(double xyz[3])
{
    double coefs[16], res[16];
    scale_coefs(xyz, coefs);
    matrix_mult(coefs, res);
    set_coefs(res);
}

void matrix4x4::do_translate(double orig[3])
{
    double coefs[16], res[16];
    translate_coefs(orig, coefs);
    matrix_mult(coefs, res);
    set_coefs(res);
}

void matrix4x4::do_translate(double x, double y, double z)
{
    double t[3]={x, y, z};
    do_translate(t);
}

bool matrix4x4::invert(matrix4x4 *r)
{
    // checks
    return invert(r->c);
}

bool matrix4x4::invert(double result[16])
{
    // determinant
    double inv_det=determinant();
    if(inv_det==0.0) ERROR_RX(false, "determinant is zero => not inversible");
    inv_det=1.0/inv_det;

    // cofactor inversion method
    for(int j=0, k=0; j<4; j++) for(int i=0; i<4; i++, k++) result[k]=inv_det*cofactor(j, i);

    // ok
    return true;
}

bool matrix4x4::invert(double c[16], double result[16])
{
    // determinant
    double inv_det=determinant(c);
    if(inv_det==0.0) ERROR_RX(false, "determinant is zero => not inversible");
    inv_det=1.0/inv_det;

    // cofactor inversion method
    for(int j=0, k=0; j<4; j++) for(int i=0; i<4; i++, k++) result[k]=inv_det*cofactor(j, i, c);

    // ok
    return true;    
}

double matrix4x4::determinant()
{
    double det=0;
    for(int i=0; i<4; i++)
	if(c[i])
	    det+=c[i]*cofactor(i, 0);

    return det;
}

double matrix4x4::determinant(double c[16])
{
    double det=0;
    for(int i=0; i<4; i++)
	if(c[i])
	    det+=c[i]*cofactor(i, 0, c);

    return det;
}

void matrix4x4::minor_matrix(double *m, double *result, int s, int ii, int jj)
{
    int k=0;
    for(int j=0; j<s; j++)
	if(j!=jj)
	    for(int i=0; i<s; i++)
		if(i!=ii)
		    result[k++]=m[i+j*s];
}

double matrix4x4::cofactor_recur(double *m, int s, int ii, int jj)
{
    // recur end
    if(s==2)
    {
	double det=m[(ii | (jj<<1)) ^ 0x3];
	return ((ii^jj)&1)?-det:det;
    }

    // minor matrix
    double minor[9];
    minor_matrix(m, minor, s, ii, jj);

    // calculate minor matrix determinant
    double det=0;
    for(int i=0; i<s-1; i++)
	if(minor[i])
	    det+=minor[i]*cofactor_recur(minor, s-1, i, 0);

    return ((ii^jj)&1)?-det:det;
}

double matrix4x4::cofactor(int ii, int jj)
{
    return cofactor_recur(c, 4, ii, jj);
}

double matrix4x4::cofactor(int ii, int jj, double c[16])
{
    return cofactor_recur(c, 4, ii, jj);
}

void matrix4x4::dump(FILE *arch, int colours, char *tab)
{
    for(int i=0; i<4; i++)
    {
	fprintf(arch, "%s", tab);
	for(int j=0; j<4; j++)
	{
	    if(colours)
	    {
		if(i==j) fprintf(arch, WHITE); else fprintf(arch, RESET);
	    }
	    fprintf(arch, "%12.6f ", c[i+(j<<2)]);
	}
	if(colours) fprintf(arch, RESET);
	fprintf(arch, "\n");
    }
}

void matrix4x4::dump_stdout()
{
    dump();
}

void matrix4x4::dump_coefs(double c[16])
{
    for(int i=0; i<4; i++)
    {
	for(int j=0; j<4; j++)
	{
	    if(i==j) printf(WHITE); else printf(RESET);
	    printf("%12.6f ", c[i+(j<<2)]);
	}
	printf(RESET"\n");
    }
}

void matrix4x4::dump_octave_stdout()
{
    FILE *arch=stdout;
    fprintf(arch, "[");
    for(int i=0; i<4; i++)
    {
	fprintf(arch, "[");
	for(int j=0; j<4; j++)
	    fprintf(arch, "%e,", c[i+(j<<2)]);
	fprintf(arch, "];");
    }
    fprintf(arch, "]\n");
}
